#include <cmath>

#include <cassert>
#include <QTextStream>
#include <QDebug>

#include "mapinfo.h"
#include "../algorithm/spiral/spiralv2.h"
#include "../algorithm/poly3/poly3.h"

#define NUM_GEOMETRY_ERASE 4

using namespace std;


MapInfo::MapInfo()
{
}

MapInfo::~MapInfo()
{
}

void MapInfo::setHeader(Header &header)
{
    m_header = header;
}

const Header & MapInfo::getHeader() const
{
    return m_header;
}

void MapInfo::setRoad(QVector<SRoad> &qvecRoad)
{
    m_qvecRoad = qvecRoad;
}

const QVector<SRoad> &MapInfo::getRoad() const
{
    return m_qvecRoad;
}

void MapInfo::setController(QVector<SController> &qvecController)
{
    m_qvecController = qvecController;
}

const QVector<SController> &MapInfo::getController() const
{
    return m_qvecController;
}

void MapInfo::setJunction(QVector<Junction> &qvecJunction)
{
    m_qvecJunction = qvecJunction;
}

const QVector<Junction> & MapInfo::getJunction() const
{
    return m_qvecJunction;
}

void printGeometryPoints(QVector<QPointF> &qvecGeometryLine)
{
    QTextStream out(stdout);
    out << "xxxxxxxxxx" << endl << "[";

    for(int i = 0; i < qvecGeometryLine.size(); ++i)
    {
        out << to_string(qvecGeometryLine[i].rx()).c_str() << ",";
    }

    out << "]" << endl << "yyyyyyyyyyy" << endl << "[";

    for(int i = 0; i < qvecGeometryLine.size(); ++i)
    {
        out << to_string(qvecGeometryLine[i].ry()).c_str() << ",";
    }

    out << "]" << endl;
}

int MapInfo::getRoadSurfaceContourPoint(const QVector<SRoad> &qvecAllRoad, RoadMapPoint &roadMapAllPoint) const
{
    for(SRoad eachRoad: qvecAllRoad)
    {
        double dRoadLength = eachRoad.length;
        RoadPoint roadPoint;
        for(int i = 0; i < eachRoad.lanes.qvecLaneSection.size(); ++i)
        {
//            if(eachRoad.id != "1976")
//            {
//                continue;
//            }

            LaneSectionPoint laneSectionPoint;
            SLaneSection eachLaneSection = eachRoad.lanes.qvecLaneSection[i];
            double dLaneStart = eachLaneSection.s;
            double dLaneEnd = 0;
            if (i+1 == eachRoad.lanes.qvecLaneSection.size())
            {
                dLaneEnd = dRoadLength;
            }
            else
            {
                dLaneEnd = eachRoad.lanes.qvecLaneSection[i+1].s;
            }

            // 车道段
            SLaneOffset eachLaneOffset;
            if (eachRoad.lanes.qvecLaneOffset.size() > 0)
            {
                eachLaneOffset = eachRoad.lanes.qvecLaneOffset.at(i);
            }

            laneSectionPoint = getLaneSectionPoint(eachLaneOffset, eachLaneSection, eachRoad.planView.qvecGeometry, dLaneStart, dLaneEnd);
            roadPoint.push_back(laneSectionPoint);
        }

        roadMapAllPoint.push_back(roadPoint);
    }

    return 0;
}

int MapInfo::getRoadLaneSectionLevelPoint(const RoadMapPoint &roadMapAllPoint, QVector<QVector<GeometryPoint> > &qvecQvecMapPoint) const
{
    for(int i = 0; i < roadMapAllPoint.size(); ++i)
    {
        // 每一个道路
        RoadPoint roadPoint = roadMapAllPoint[i];
        QVector<GeometryPoint> qvecRoadPoint;

        for(int j = 0; j < roadPoint.size(); ++j)
        {
            // 每一个车道段
            LaneSectionPoint laneSectionPoint = roadPoint[j];
            QVector<GeometryPoint> qvecAllGeometryPoint;

            for(int k = 0; k < laneSectionPoint.size(); ++k)
            {
                // 每一个几个元素的点
                GeometryPoint geometryPoint = laneSectionPoint[k].first;

                if(laneSectionPoint.size() > 1 && k < laneSectionPoint.size() - 1)
                {
                    for(int n = 0; n < geometryPoint.size(); ++n)
                    {
//                        assert(geometryPoint[n].size()-3 > 0);
                        if (geometryPoint[n].size() > NUM_GEOMETRY_ERASE)
                        {
                            geometryPoint[n].erase(geometryPoint[n].begin() + geometryPoint[n].size() - NUM_GEOMETRY_ERASE, geometryPoint[n].end());
                        }
                    }
                }

                qvecAllGeometryPoint.push_back(geometryPoint);
            }

            // 合并一个车道段所有几何元素的点
            GeometryPoint geometryEachLaneSection;
            mergeAllRelativelyLaneSection(geometryEachLaneSection, qvecAllGeometryPoint);

            qvecRoadPoint.push_back(geometryEachLaneSection);
        }

        qvecQvecMapPoint.push_back(qvecRoadPoint);
    }

    return 0;
}

void getGeometryIndex(QVector<SGeometry> &qvecGeometry, int &iStartIndex, int &iEndIndex, double dLaneStart, double dLaneEnd)
{
    SGeometry eachGeometry;

    for (iStartIndex = 0; iStartIndex < qvecGeometry.size(); ++iStartIndex)
    {
        eachGeometry = qvecGeometry[iStartIndex];
        if (eachGeometry.s > dLaneStart)
            break;
    }

    for (iEndIndex = 0; iEndIndex < qvecGeometry.size(); ++iEndIndex)
    {
        eachGeometry = qvecGeometry[iEndIndex];
        if (eachGeometry.s >= dLaneEnd)
            break;
    }

    iStartIndex -= 1;
    iEndIndex -= 1;
}

LaneSectionPoint MapInfo::getLaneSectionPoint(SLaneOffset &laneOffset, SLaneSection &laneSection, QVector<SGeometry> &qvecGeometry, double dLaneStart, double dLaneEnd) const
{
    // 获取当前车道段所在几何元素的起止区间
    int I,J;
    getGeometryIndex(qvecGeometry, I, J, dLaneStart, dLaneEnd);

    LaneSectionPoint laneSectionPoint;
    double dRelativelyStart, dRelativelyEnd;
    for (int k = I; k <= J; ++k)
    {
        QVector<QVector<QPointF> > qvecQvecEachRelativelyLaneSection;
        SGeometry eachGeometry = qvecGeometry[k];

        if (0 == (J - I))
        {
            dRelativelyStart = dLaneStart - eachGeometry.s;
            dRelativelyEnd = dLaneEnd - eachGeometry.s;
        }
        else if (1 <= (J - I))
        {
            if (k == I)
            {
                dRelativelyStart = dLaneStart - eachGeometry.s;
                dRelativelyEnd = eachGeometry.length;
            }
            else if (k == J)
            {
                dRelativelyStart = 0;
                dRelativelyEnd = dLaneEnd - eachGeometry.s;
            }
            else
            {
                dRelativelyStart = 0;
                dRelativelyEnd = eachGeometry.length;
            }
        }
        else
        {
            assert("other case");
        }

        // 车道段对应的几何元素，dRelativelyStart，dRelativelyEnd 几何元素偏移
        qvecQvecEachRelativelyLaneSection = getGeometryData(laneOffset, laneSection, eachGeometry, dRelativelyStart, dRelativelyEnd);

//        QTextStream out(stdout);
//        out << "======================================================================================" << endl;
//        for(auto each_lane : qvecQvecEachRelativelyLaneSection)
//        {
//            printGeometryPoints(each_lane);
//        }

        laneSectionPoint.push_back(std::make_pair(qvecQvecEachRelativelyLaneSection, k));
    }

    // 将相对车道段合并  状态栏参数基准到几何元素
//    QVector<QVector<QPointF> > qvecQvecEachLaneSection;
//    mergeAllRelativelyLaneSection(qvecQvecEachLaneSection, qvecQvecQvecAllRelativelyLaneSection);

    return laneSectionPoint;
}

QVector<QVector<QPointF> > MapInfo::getGeometryData(const SLaneOffset &laneOffset, const SLaneSection &laneSection, const SGeometry &geometry, double dStartDS, double dEndDS) const
{
    // 地理几何元素s 减 车道段s，得到相对车道段起始点的距离
    // 如果几何元素距离大于车道段距离，以几何元素的相对值为起始
    // 如果几何元素距离小于车道段距离，以车道段的相对值为起始
    double dSOffset = 0.0; // 几何元素相对于当前车道段的偏移
    if (geometry.s > laneSection.s)
    {
        dSOffset = geometry.s - laneSection.s;
    }

    // 中心车道
    QVector<QVector<QPointF> > qvecQvecCenterLanePoint;
    SLane centerLane = laneSection.center.qvecLane[0];
    getGeometryLaneData(geometry, laneOffset, centerLane, dSOffset, dStartDS, dEndDS, qvecQvecCenterLanePoint);

    // 左侧车道
    QVector<QVector<QPointF> > qvecQvecLeftLanesPoint;
    qvecQvecLeftLanesPoint.push_back(qvecQvecCenterLanePoint[0]);
    for (int i = laneSection.left.qvecLane.size() - 1; i >= 0; --i)
    {
        SLane eachLane = laneSection.left.qvecLane[i];
        getGeometryLaneData(geometry, laneOffset, eachLane, dSOffset, dStartDS, dEndDS, qvecQvecLeftLanesPoint);
    }

    // 右侧车道
    QVector<QVector<QPointF> > qvecQvecRightLanesPoint;
    qvecQvecRightLanesPoint.push_back(qvecQvecCenterLanePoint[0]);
    for (int i = 0; i < laneSection.right.qvecLane.size(); ++i)
    {
        SLane eachLane = laneSection.right.qvecLane[i];
        getGeometryLaneData(geometry, laneOffset, eachLane, dSOffset, dStartDS, dEndDS, qvecQvecRightLanesPoint);
    }

    // 调整顺序和车道段车道顺序一致
    QVector<QVector<QPointF> > qvecQvecAllLanesPoint;
    for (int i = qvecQvecLeftLanesPoint.size() - 1; i >= 0; --i)
    {
        qvecQvecAllLanesPoint.push_back(qvecQvecLeftLanesPoint[i]);
    }

    // 从下标1开始过滤掉多余的0车道
    for (int j = 1; j < qvecQvecRightLanesPoint.size(); ++j)
    {
        qvecQvecAllLanesPoint.push_back(qvecQvecRightLanesPoint[j]);
    }

    return qvecQvecAllLanesPoint;
}

void MapInfo::mergeAllRelativelyLaneSection(QVector<QVector<QPointF> > &qvecQvecEachLaneSection, const QVector<QVector<QVector<QPointF> > > &qvecQvecQvecAllRelativelyLaneSection) const
{
    for(auto eachRelativelyLaneSection: qvecQvecQvecAllRelativelyLaneSection)
    {
        for(int i = 0; i < eachRelativelyLaneSection.size(); ++i)
        {
            if (qvecQvecEachLaneSection.size() < eachRelativelyLaneSection.size())
            {
                qvecQvecEachLaneSection.push_back(eachRelativelyLaneSection[i]);
            }
            else
            {
                qvecQvecEachLaneSection[i] += eachRelativelyLaneSection[i];
            }
        }

    }
}

void MapInfo::getGeometryLaneData(const SGeometry &geometry, const SLaneOffset &laneOffset, const SLane &lane, double dSOffset, double dStartDS, double dEndDS, QVector<QVector<QPointF>> &qvecQvecLanesPoint) const
{
    if (geometry.type == XML_LINE)
    {
       getGeometryLineData(dSOffset, dStartDS, dEndDS, 1, geometry, laneOffset, lane, qvecQvecLanesPoint);
    }
    else if (geometry.type == XML_SPIRAL)
    {
        getGeometrySpiralData(dSOffset, dStartDS, dEndDS, 1, geometry, laneOffset, lane, qvecQvecLanesPoint);
    }
    else if (geometry.type == XML_ARC)
    {
        getGeometryArcData(dSOffset, dStartDS, dEndDS, 1, geometry, laneOffset, lane, qvecQvecLanesPoint);
    }
    else if (geometry.type == XML_POLY3)
    {
        getGeometryPoly3Data(dSOffset, dStartDS, dEndDS, 1, geometry, lane, qvecQvecLanesPoint);
    }
}

void getLineReferenceData(double dXStart, double dYStart, double dHdg, double dSOffset, double dDistance, double dSOffsetDistance, const SLaneOffset &laneOffset, QVector<QPointF> &qvecGeometryLine)
{
    // 参考线偏移
    double dLaneOffset = laneOffset.coefficients.a + laneOffset.coefficients.b * (dSOffset+dSOffsetDistance) + laneOffset.coefficients.c * pow((dSOffset+dSOffsetDistance), 2) + laneOffset.coefficients.d * pow((dSOffset+dSOffsetDistance), 3);

    QPointF qpointf;
    qpointf.setX(dXStart + dDistance * cos(dHdg) - dLaneOffset * sin(dHdg));
    qpointf.setY(dYStart + dDistance * sin(dHdg) + dLaneOffset * cos(dHdg));
    qvecGeometryLine.push_back(qpointf);
}

void getLineLanesData(Width &laneWidth, double dSOffset, QVector<QPointF> &qvecPrelane, double dDistance, double iIndex, double dSgn, double dHdg, QVector<QPointF> &qvecGeometryLine)
{
    // 车道宽度
    double dWidth = laneWidth.coefficients.a + laneWidth.coefficients.b * (dSOffset+dDistance) + laneWidth.coefficients.c * pow((dSOffset+dDistance), 2) + laneWidth.coefficients.d * pow((dSOffset+dDistance), 3);

    QPointF qpointf;
    qpointf.setX(qvecPrelane[iIndex].x() - dSgn * dWidth * sin(dHdg));
    qpointf.setY(qvecPrelane[iIndex].y() + dSgn * dWidth * cos(dHdg));
    qvecGeometryLine.push_back(qpointf);
}

void MapInfo::getGeometryLineData(double dSOffset, double dStartDS, double dEndDS, double dStepSize, const SGeometry &geometry, const SLaneOffset &laneOffset, const SLane &lane, QVector<QVector<QPointF>> &qvecQvecLanePoint) const
{
    // 原始数据
    double dXStart = geometry.x;
    double dYStart = geometry.y;
    double dHdg = geometry.hdg;
    double dLaneId = stod(lane.id);
    Width laneWidth = lane.width;

    // 转换数据
    double dSgn = sgn(dLaneId);
    double dDistance = dEndDS - dStartDS;

    // 每条车道的点
    QVector<QPointF> qvecGeometryLine;

    if (qvecQvecLanePoint.size() == 0)
    {
        for(double ds = dStartDS, i = 0; ds < dEndDS; ds += dStepSize, ++i)
        {
            getLineReferenceData(dXStart, dYStart, dHdg, dSOffset, ds, i, laneOffset, qvecGeometryLine);
        }

        getLineReferenceData(dXStart, dYStart, dHdg, dSOffset, dEndDS, dDistance, laneOffset, qvecGeometryLine);
    }
    else
    {
        // 当宽度为0可以考虑略过循环 20200304 bjd
        QVector<QPointF> qvecPrelane = qvecQvecLanePoint[qvecQvecLanePoint.size() - 1];
        if (isZero(laneWidth.coefficients.a) && isZero(laneWidth.coefficients.b) && isZero(laneWidth.coefficients.c) && isZero(laneWidth.coefficients.d))
        {
            qvecQvecLanePoint.push_back(qvecPrelane);
            return;
        }

        for(int i = 0; i < qvecPrelane.size() - 1; ++i)
        {
            getLineLanesData(laneWidth, dSOffset, qvecPrelane, i, i, dSgn, dHdg, qvecGeometryLine);
        }

        getLineLanesData(laneWidth, dSOffset, qvecPrelane, dDistance, qvecPrelane.size() - 1, dSgn, dHdg, qvecGeometryLine);
    }

    qvecQvecLanePoint.push_back(qvecGeometryLine);
}

void MapInfo::getGeometrySpiralData(double dSOffset, double dStartDS, double dEndDS, double dStepSize, const SGeometry &geometry, const SLaneOffset &laneOffset, const SLane &lane, QVector<QVector<QPointF> > &qvecQvecLanePoint) const
{
    // 原始数据
    double dXCoordinate = geometry.x;
    double dYCoordinate = geometry.y;
    double dHdg = geometry.hdg;
    double dLength = geometry.length;
    double dCurvatureStart = geometry.extend.a;
    double dCurvatureEnd = geometry.extend.b;
    double dLaneId = stod(lane.id);
    Width laneWidth = lane.width;

    // 转换数据
    double dSgn = sgn(dLaneId);

    // 每条车道的点
    QVector<QPointF> qvecGeometrySpiral;

    static PointHdg pointHdg;
    if (qvecQvecLanePoint.size() == 0)
    {
        SpiralV2 *pSpiral = nullptr;
        pointHdg = pSpiral->geometrySpiralData(dXCoordinate, dYCoordinate, dHdg, dLength, dCurvatureStart, dCurvatureEnd);
        qvecGeometrySpiral = pointHdg.qvecPointData;
    }
    else
    {
        // 当宽度为0可以考虑略过循环 20200304 bjd
        QVector<QPointF> qvecPrelane = qvecQvecLanePoint[qvecQvecLanePoint.size() - 1];
        if (isZero(laneWidth.coefficients.a) && isZero(laneWidth.coefficients.b) && isZero(laneWidth.coefficients.c) && isZero(laneWidth.coefficients.d))
        {
            qvecQvecLanePoint.push_back(qvecPrelane);
            return;
        }

        for(int i = 0; i < qvecPrelane.size(); ++i)
        {
            getLineLanesData(laneWidth, dSOffset, qvecPrelane, pointHdg.qvecSData[i], i, dSgn, pointHdg.qvecHdgData[i], qvecGeometrySpiral);
        }
    }

    qvecQvecLanePoint.push_back(qvecGeometrySpiral);
}

// 弧型几何元素的参考线
void getArcReferenceData(double dR, double dHdg, double dDistance, double dXStart, double dYStart, QVector<QPointF> &qvecGeometryArc)
{
    double dOffsetX = dR * (sin(dHdg) - sin(dHdg - dDistance / dR));  //x方向偏移
    double dOffsetY = dR * (cos(dHdg - dDistance / dR) - cos(dHdg));  //y方向偏移

    QPointF qpointf;
    qpointf.setX(dXStart + dOffsetX);
    qpointf.setY(dYStart + dOffsetY);
    qvecGeometryArc.push_back(qpointf);
}

void getArcLanesData(Width &laneWidth, double dSOffset, double dLaneSectionOffset, double dR, double dHdg, double dSgnID, double dGeometryOffset, int iIndex, QVector<QPointF> &preLane, QVector<QPointF> &qvecGeometryArc)
{
    double dWidth = laneWidth.coefficients.a + laneWidth.coefficients.b * (dSOffset+dLaneSectionOffset) + laneWidth.coefficients.c * pow((dSOffset+dLaneSectionOffset), 2) + laneWidth.coefficients.d * pow((dSOffset+dLaneSectionOffset), 3);

    double dHdgOffset = dGeometryOffset / dR;  //角度的偏移量， 距离是几何元素的相对距离
    double dHdgCorrespond = dHdg - dHdgOffset;  //点的切角大小

    double dCurrentX = preLane[iIndex].x() - dSgnID * dWidth * sin(dHdgCorrespond);  //求弧线上点的对应点的x坐标
    double dCurrentY = preLane[iIndex].y() + dSgnID * dWidth * cos(dHdgCorrespond);  //求弧线上点的对应点的y坐标

    QPointF qpointf;
    qpointf.setX(dCurrentX);
    qpointf.setY(dCurrentY);
    qvecGeometryArc.push_back(qpointf);
}

void MapInfo::getGeometryArcData(double dSOffset, double dStartDS, double dEndDS, double dStepSize, const SGeometry &geometry, const SLaneOffset &laneOffset, const SLane &lane, QVector<QVector<QPointF> > &qvecQvecLanePoint) const
{
    double dXStart = geometry.x;
    double dYStart = geometry.y;
    double dHdg = geometry.hdg;
    double dCurvature = geometry.extend.a;
    double dLaneID = stod(lane.id);
    Width laneWidth = lane.width;

    double dSgnID = sgn(dLaneID);
    double dR = -1 / dCurvature;

    QVector<QPointF> qvecGeometryArc;
    static double dCurrentR = dR;

    if (qvecQvecLanePoint.size() == 0)
    {
        for(double ds = dStartDS; ds < dEndDS; ds += dStepSize)
        {
            getArcReferenceData(dR, dHdg, ds, dXStart, dYStart, qvecGeometryArc);
        }

        // 终点坐标
        getArcReferenceData(dR, dHdg, dEndDS, dXStart, dYStart, qvecGeometryArc);
    }
    else
    {
        // 当宽度为0可以考虑略过循环 20200304 bjd
        QVector<QPointF> preLane = qvecQvecLanePoint[qvecQvecLanePoint.size() - 1];
        if (isZero(laneWidth.coefficients.a) && isZero(laneWidth.coefficients.b) && isZero(laneWidth.coefficients.c) && isZero(laneWidth.coefficients.d))
        {
            qvecQvecLanePoint.push_back(preLane);
            return;
        }

        // R每次减去车道宽度，小于0就返回前一层的点
//        double dWidth = laneWidth.coefficients.a + laneWidth.coefficients.b * (dSOffset) + laneWidth.coefficients.c * pow((dSOffset), 2) + laneWidth.coefficients.d * pow((dSOffset), 3);
//        dCurrentR -= dWidth;
//        if(dCurrentR < 0)
//        {
//            qvecQvecLanePoint.push_back(preLane);
//            return;
//        }

        // 传入几何元素偏移和车道段偏移
        for(int i = 0; i < preLane.size() - 1; ++i)
        {
            // 车道段偏移的距离
            double dLaneSectionOffset = i;
            // 几何元素偏移的距离
            double dGeometryOffset = dStartDS + i;
            getArcLanesData(laneWidth, dSOffset, dLaneSectionOffset, dR, dHdg, dSgnID, dGeometryOffset, i, preLane, qvecGeometryArc);
        }

        double dLaneSectionOffset = dEndDS - dStartDS;
        double dGeometryOffset = dEndDS;
        getArcLanesData(laneWidth, dSOffset, dLaneSectionOffset, dR, dHdg, dSgnID, dGeometryOffset, preLane.size()-1, preLane, qvecGeometryArc);
    }

    qvecQvecLanePoint.push_back(qvecGeometryArc);
}

void getPoly3ReferenceData(const SGeometry &geometry, double dDistance, double dHdg, double dXStart, double dYStart, QVector<QPointF> &qvecGeometryPoly)
{
    double dVLocal = geometry.extend.a + geometry.extend.b * dDistance + geometry.extend.c * pow(dDistance, 2) + geometry.extend.d * pow(dDistance, 3);
    // s和t进行旋转平移
    double dSChange = dDistance * cos(dHdg) - dVLocal * sin(dHdg);
    double dTChange = dDistance * sin(dHdg) + dVLocal * cos(dHdg);

    QPointF qpoint;
    qpoint.setX(dXStart + dSChange);
    qpoint.setY(dYStart + dTChange);
    qvecGeometryPoly.push_back(qpoint);
}

void getPoly3LanesData(const Width &laneWidth, double dSOffset, double dLaneSectionOffset, const SGeometry &geometry, double dGeometryOffset,int iIndex, double dSgnID, double dHdg, QVector<QPointF> &qvecPrelane, QVector<QPointF> &qvecGeometryPoly)
{
    double dWidth = laneWidth.coefficients.a + laneWidth.coefficients.b * (dSOffset+dLaneSectionOffset) + laneWidth.coefficients.c * pow((dSOffset+dLaneSectionOffset), 2) + laneWidth.coefficients.d * pow((dSOffset+dLaneSectionOffset), 3);

//    double dNewB = geometry.extend.b * geometry.extend.a;
//    double dNewC = geometry.extend.c * geometry.extend.b;
//    double dNewD = geometry.extend.d * geometry.extend.c;
//    double dTangent = dNewB + dNewC * dGeometryOffset + dNewD * pow(dGeometryOffset, 2);
//    double dTangent = geometry.extend.b + 2 * geometry.extend.c * dGeometryOffset + 3 * geometry.extend.d * pow(dGeometryOffset, 2);

    // 车道线处位置
    double dRoadMarkX = qvecPrelane[iIndex].x() - dSgnID * dWidth * sin(dHdg/* + dTangent*/);
    double dRoadMarkY = qvecPrelane[iIndex].y() + dSgnID * dWidth * cos(dHdg/* + dTangent*/);

    QPointF qpoint;
    qpoint.setX(dRoadMarkX);
    qpoint.setY(dRoadMarkY);
    qvecGeometryPoly.push_back(qpoint);
}

void MapInfo::getGeometryPoly3Data(double dSOffset, double dStartDS, double dEndDS, double dStepSize, const SGeometry &geometry, const SLane &lane, QVector<QVector<QPointF> > &qvecQvecLanePoint) const
{
    double dXStart = geometry.x;
    double dYStart = geometry.y;
    double dHdg = geometry.hdg;
    double dLaneID = stod(lane.id);
    Width laneWidth = lane.width;

    double dSgnID = sgn(dLaneID);
    QVector<QPointF> qvecGeometryPoly;

    static PointHdg pointHdg;
    if (qvecQvecLanePoint.size() == 0)
    {
//        for(double ds = dStartDS; ds < dEndDS; ds += dStepSize)
//        {
//            getPoly3ReferenceData(geometry, ds, dHdg, dXStart, dYStart, qvecGeometryPoly);
//        }

//        getPoly3ReferenceData(geometry, dEndDS, dHdg, dXStart, dYStart, qvecGeometryPoly);
        Poly3 *pPoly3 = nullptr;
        pointHdg = pPoly3->geometryPoly3Data(dXStart, dYStart, dHdg, geometry.extend.a, geometry.extend.b, geometry.extend.c, geometry.extend.d, dStartDS, dEndDS);
        qvecGeometryPoly = pointHdg.qvecPointData;
    }
    else
    {
        // 当宽度为0可以考虑略过循环 20200304 bjd
        QVector<QPointF> qvecPrelane = qvecQvecLanePoint[qvecQvecLanePoint.size() - 1];
        if (isZero(laneWidth.coefficients.a) && isZero(laneWidth.coefficients.b) && isZero(laneWidth.coefficients.c) && isZero(laneWidth.coefficients.d))
        {
            qvecQvecLanePoint.push_back(qvecPrelane);
            return;
        }

//        for(int i = 0; i < qvecPrelane.size() - 1; ++i)
//        {
//            // 车道段偏移的距离
//            double dLaneSectionOffset = i;
//            // 几何元素偏移的距离
//            double dGeometryOffset = dStartDS + i;
//            getPoly3LanesData(laneWidth, dSOffset, dLaneSectionOffset, geometry, dGeometryOffset, i, dSgnID, dHdg, qvecPrelane, qvecGeometryPoly);
//        }

//        double dLaneSectionOffset = dEndDS - dStartDS;
//        double dGeometryOffset = dEndDS;
//        getPoly3LanesData(laneWidth, dSOffset, dLaneSectionOffset, geometry, dGeometryOffset, qvecPrelane.size() - 1, dSgnID, dHdg, qvecPrelane, qvecGeometryPoly);

        for(int i = 0; i < qvecPrelane.size() -1 ; ++i)
        {
            // 车道段偏移的距离
            double dLaneSectionOffset = i;
            // 几何元素偏移的距离
            double dGeometryOffset = dStartDS + i;
            getPoly3LanesData(laneWidth, dSOffset, dLaneSectionOffset, geometry, pointHdg.qvecSData[i], i, dSgnID, pointHdg.qvecHdgData[i], qvecPrelane, qvecGeometryPoly);
        }

        double dLaneSectionOffset = dEndDS - dStartDS;
        double dGeometryOffset = dEndDS;
        getPoly3LanesData(laneWidth, dSOffset, dLaneSectionOffset, geometry, pointHdg.qvecSData[pointHdg.qvecSData.size()-1], qvecPrelane.size() - 1, dSgnID, pointHdg.qvecHdgData[pointHdg.qvecHdgData.size()-1], qvecPrelane, qvecGeometryPoly);
    }

    qvecQvecLanePoint.push_back(qvecGeometryPoly);
}

//符号函数的定义,传入一个变量,如果变量为正数,返回1;如果变量为负数,返回-1;如果变量为0,返回0
double MapInfo::sgn(double dSgnVar) const
{
    double dSgnValue;

    if (dSgnVar > 0.0)
    {
        dSgnValue = 1;
    }
    else if (dSgnVar < 0.0)
    {
        dSgnValue = -1;
    }
    else if (dSgnVar <= std::numeric_limits<double>::epsilon() && dSgnVar >= -std::numeric_limits<double>::epsilon())
    {
        dSgnValue = 0;
    }
    else
    {
        assert("MapInfo::sgn error case");
    }

    return dSgnValue;
}

template<typename T>
bool MapInfo::isZero(T value)
{
    if (value <= std::numeric_limits<T>::epsilon() && value >= -std::numeric_limits<T>::epsilon())
        return true;
    else
        return false;
}
